#include <msp430x16x.h>

#include "../timing/timing.h"
#include "../adc/adc.h"
#include "adcs.h"

void ADCS_init() {
  P5DIR|=0x24;
}

void ADCS_reset() {
  P5DIR|=0x40;
  P5OUT|=0x40;
  waitms(100);
  P5OUT&=0x40;
}

int ADCS(int axis) { //0=x 1=y 2=z
  int attitude[3];
  int i;

  adcEnable(0);
  /* Select ADCS channels using port 5 and store samples from ADC to attitude array */

  P5OUT|=0x10; P5OUT&=0xFB; //X
  for (i=0; i<10000; i++);
  //WAITUS(641);
  attitude[0]=adcSample(5,1);

  P5OUT&=0xEF; //Y
  for (i=0; i<10000; i++);
  //WAITUS(641);
  attitude[1]=adcSample(5,1);

  P5OUT|=0x04; //Z
  for (i=0; i<10000; i++);
  //WAITUS(641);
  attitude[2]=adcSample(5,1);

  P5OUT&=0xFB; //Y
  WAITUS(641);
  adcSample(7,1);
  
  adcDisable();

  return attitude[axis];
}
